#!/usr/bin/env python3
import rospy
import math
import actionlib
import threading
import tf
from std_msgs.msg import Int32, String
from actionlib_msgs.msg import GoalStatus
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_from_euler
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from sound_play.libsoundplay import SoundClient
# 配置参数
ACTION_MOVE_BASE = "move_base"
FRAME_MAP = "map"
FRAME_BASE_LINK = "base_link"

class SimpleNavigation:
    def __init__(self):
        rospy.on_shutdown(self.clean_up)
        rospy.init_node('simple_navigation')
        self.sound_client = SoundClient()
        rospy.loginfo("SoundClient initialized. Waiting for sound_play node...")
        rospy.sleep(1)  # 给sound_play节点启动时间
        self.voice = 'voice_kal_diphone'  # 设置语音引擎
        # 预设目标位置 (x, y, yaw角度)
        self.targets = {
            "green": [3.0, 1.0, 0.0],    # 位置1
            "red": [3.0, 2.0, 0.0],    # 位置2
            "black": [3.0, 3.0, 0.0] # 位置3
        }
        
        # 状态变量
        self.detected_template = None  # 存储检测到的模板名称
        self.navigation_enabled = False  # 导航使能标志
        
        # 动作客户端初始化
        self.move_base = actionlib.SimpleActionClient(ACTION_MOVE_BASE, MoveBaseAction) 
        rospy.loginfo("Waiting for move_base action server...") 
        if not self.move_base.wait_for_server(rospy.Duration(120)):
            rospy.logerr("连接move_base动作服务器超时！")
            rospy.signal_shutdown("动作服务器不可用")
            return
        rospy.loginfo("已连接到move_base服务器")
        
        # TF监听器
        self.tf_listener = tf.TransformListener()
        rospy.sleep(1.0)  # 初始化时间
        
        # 订阅语音指令
        rospy.Subscriber("/voice_controller/commands", Int32, self.voice_command_callback)
        
        # 订阅模板识别结果
        rospy.Subscriber("/color_detector_node/color", String, self.template_callback)
        
        rospy.loginfo("简单导航系统已启动")
        rospy.loginfo("等待指令...")
        
        # 启动状态检查定时器
        self.status_timer = rospy.Timer(rospy.Duration(0.5), self.check_navigation_status)

    def voice_command_callback(self, msg):
        command = msg.data
        rospy.loginfo(f"收到语音指令: {command}")
        
        if command == 3:  # 导航指令
            rospy.loginfo("导航指令使能")
            self.navigation_enabled = True
            
            # 如果已有检测到的模板，则启动导航
            if self.detected_template:
                rospy.loginfo(f"导航到已检测的模板: {self.detected_template}")
                threading.Thread(
                    target=self.goto, 
                    args=(self.targets[self.detected_template],)
                ).start()
            else:
                rospy.logwarn("未检测到目标模板，等待识别结果...")
                
        elif command == 0:  # 停止指令
            rospy.loginfo("停止指令")
            self.navigation_enabled = False
            self.move_base.cancel_all_goals()
            rospy.loginfo("已取消所有目标")

    # 模板识别回调函数
    def template_callback(self, msg):
        word = msg.data.lower().strip()
        rospy.loginfo(f"识别到模板: {word}")
        
        if word in self.targets:
            self.detected_template = word
            rospy.loginfo(f"已存储模板: {word}")
            
            # 如果导航已使能，立即启动导航
            if self.navigation_enabled:
                rospy.loginfo(f"导航到 '{word}' 位置")
                threading.Thread(
                    target=self.goto, 
                    args=(self.targets[word],)
                ).start()
        else:
            rospy.logwarn(f"未定义的模板: {word}")

    def check_navigation_status(self, event):
        """定期检查导航状态并处理待命模板"""
        if self.navigation_enabled and self.detected_template:
            rospy.loginfo(f"导航已使能，启动检测到的模板导航: {self.detected_template}")
            threading.Thread(
                target=self.goto, 
                args=(self.targets[self.detected_template],)
            ).start()
            self.detected_template = None  # 清除已处理的模板

    def goto(self, target):
        """导航到目标位置并输出位置信息"""
        # 记录目标位置
        rospy.loginfo("[导航] 目标位置: x=%.3f, y=%.3f, yaw=%.2f°",
                     target[0], target[1], target[2])
        
        try:
            # 转换为目标姿态
            yaw_rad = target[2] * math.pi / 180.0
            quat = quaternion_from_euler(0, 0, yaw_rad)
            
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = FRAME_MAP
            goal.target_pose.header.stamp = rospy.Time.now()
            goal.target_pose.pose.position = Point(target[0], target[1], 0)
            goal.target_pose.pose.orientation = Quaternion(*quat)
            
            # 发送目标
            self.move_base.send_goal(goal)
            rospy.loginfo("已发送导航目标到move_base")
            
            # 等待结果（最长30秒）
            rospy.loginfo('等待导航结果...')
            success = self.move_base.wait_for_result(rospy.Duration(30))
            
            if success:
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("导航完成")
                else:
                    rospy.logwarn("导航失败，状态码: %d", state)
            else:
                rospy.logwarn("导航超时!")
                
        except Exception as e:
            rospy.logerr("导航出错: %s", str(e))
        
        # 获取并输出当前位置
        self.print_current_position()
        self.navigation_enabled = False 
    def print_current_position(self):
        """获取并输出当前机器人的位置和朝向"""
        try:
            # 尝试获取变换 (map到base_link)
            (trans, rot) = self.tf_listener.lookupTransform(
                FRAME_MAP, 
                FRAME_BASE_LINK, 
                rospy.Time(0)
            )
            
            # 提取位置
            x, y, z = trans
            # 从四元数提取偏航角
            (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rot)
            yaw_deg = math.degrees(yaw)
            
            rospy.loginfo("[当前位置] X=%.3f, Y=%.3f, Yaw=%.2f°", x, y, yaw_deg)
            tts_message = f"我到达了相应的回收处"
            
              
            # 使用sound_play播放语音
            self.sound_client.say(tts_message, self.voice)
            rospy.loginfo(f"Playing: {tts_message}")
            return (x, y, yaw_deg)
            
            
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logerr("无法获取当前位置: %s", str(e))
            return None
        except Exception as e:
            rospy.logerr("位置检查出错: %s", str(e))
            return None

    def clean_up(self):
        rospy.loginfo("导航节点关闭中...")
        self.status_timer.shutdown()  # 停止定时器
        self.move_base.cancel_all_goals()
        rospy.loginfo("清理完成")
        
if __name__ == '__main__':
    try:
        nav = SimpleNavigation()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.logerr("节点意外终止")
